#include <opencv2/opencv.hpp>
#include <jni.h>
#include "tianjin_calmcar_calmcar_adas_AdasCarLaneWarper.h"
#include <string>
#include <iostream>
#include <fstream>
#include <vector>
#include <time.h>
#include <unistd.h>
#include"json.h"
#ifndef SIMD_OPENCV_ENABLE
#define SIMD_OPENCV_ENABLE
#endif

#include "SimdDetection.hpp"
typedef Simd::Detection<Simd::Allocator> Detection;
using namespace std;
using namespace cv;

Detection detection;
Detection detectionHalf;
Detection detectionSmall;

float VP_NATIVE_X0=-1 ;
float VP_NATIVE_Y0=-1;
int   isInit=false;
int   isInitHalf=false;

int CAR_DETECT_OPEN=0;
int LANE_DETECT_OPEN=0;
//--------------------------------------------车道线head------------------------------start
#include "road_marking_detection.h"
#include "lane_const_parameter.h"
#include "lane_line_struct.h"
#include "data_frame.h"
#include <vector>
#include <time.h>
#include <unistd.h>
#include <unistd.h>
#include "json/json.h"
#include "MD5.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>

using  namespace rmd;
using namespace std;
rmd::RoadMarkingDetection *roadMarkingDetection=NULL;
int licenceEnable=0;
cv::VideoWriter out;
//--------------------------------------------车道线head------------------------------end
cv::Rect simd2cv(Simd::Detection<Simd::Allocator>::Rect& rect)
{
    cv::Rect result;
    result.x=rect.left;
    result.y=rect.top;
    result.width=rect.right-rect.left;
    result.height=rect.bottom-rect.top;
    return result;
}
Json::Value test_it(Mat str);
Json::Value test_it_half(Mat str);
Json::Value  test_lane(Mat str);

Json::Value  test_move(Mat str);

JNIEXPORT jint JNICALL Java_tianjin_calmcar_calmcar_1adas_AdasCarLaneWarper_carInitNew
        (JNIEnv *, jobject ){
    return 1;
}
JNIEXPORT void JNICALL Java_tianjin_calmcar_calmcar_1adas_AdasCarLaneWarper_carStartNew
        (JNIEnv *, jobject)
{
    chdir("/mnt/sdcard/test/");
    detection.Load("car_lbp_init_config.xml");
    detectionHalf.Load("car_lbp_init_config_half.xml");
    detectionSmall.Load("car_lbp_init_config.xml");
}



Json::Value  test_it(Mat tmp_frame)
{

    cv::Mat frame;
    float ratio=0.4;
    if (tmp_frame.empty())
        return "";
    int height=tmp_frame.rows;
    int width=tmp_frame.cols;


    //roi区域保持一致
    //Rect rect = new Rect(mRgba .width() / 3, mRgba .height() / 3, mRgba.width() / 3, mRgba.height() *5/ 8);
    int left=width / 4+30;//1/3-->1/4
    int top=height *3/16;
    int new_width=width *1/2-60 ;//1/3-->1/2
    int new_height=height *11/ 16;//5/8-->6/8

    //转换-算法
    if(VP_NATIVE_X0>0){
        left=width / 4+30- (width/2-VP_NATIVE_X0);
        if(left<0) left=0;
        if(left+new_width> width) {
            left=width-new_width-5;
        }
    }
    if(VP_NATIVE_Y0>0 ){
        top=height*3  /16-( height/2-VP_NATIVE_Y0);
        if(top<0) top=0;
        if(top+new_height>height){
            top=height-new_height-5;
        }
    }

    cv::Rect roi(left,top,new_width,new_height);
    cv::Mat roi_image=tmp_frame(roi);
    tmp_frame.release();
    cv::resize(roi_image,roi_image,cv::Size(),ratio,ratio);
    Detection::View image = roi_image;
    //1.15--1.18---1.2
//    if(!isInit){
//        isInit=true;
//        detection.Init(image.Size(),1.16,Detection::Size(8,8),Detection::Size(300,300),Detection::View(),2);
//    }
    //1.15--1.18---1.2
    if(!isInit){
        isInit=true;
        detection.Init(image.Size(),1.17,Detection::Size(7,7),Detection::Size(200,200),Detection::View(),4);
    }
    double timeStart = (double)getTickCount();
    Detection::Objects objects;
    detection.Detect(image, objects);

    double Time = ((double)getTickCount() - timeStart) / getTickFrequency() * 1000;
    Json::Value root;
    Json::Value jVPointArray;
    for(int i=0;i<objects.size();i++)
    {
        cv::Rect bbox=simd2cv(objects[i].rect);
        bbox.x/=ratio;
        bbox.y/=ratio;
        bbox.width/=ratio;
        bbox.height/=ratio;

        int tl_x = bbox.tl().x+left;
        int tl_y = bbox.tl().y+top;
        int br_x = bbox.br().x+left;
        int br_y = bbox.br().y+top;
        Json::Value jVPoint;

        jVPoint["x0"]=tl_x;
        jVPoint["y0"]=tl_y;
        jVPoint["x1"]=br_x;
        jVPoint["y1"]=br_y;
        jVPointArray.append(jVPoint);
    }
    root["carPoint"] = jVPointArray;
    root["time"] = Time;
//    std::string out= root.toStyledString();
    return root;
}

Json::Value  test_it_for_big_and_small(Mat tmp_frame)
{

    cv::Mat frame;
    float ratio=0.8;
    float ratio0=0.8;
    if (tmp_frame.empty())
        return "";
    int height=tmp_frame.rows;
    int width=tmp_frame.cols;

    //roi区域保持一致
    //Rect rect = new Rect(mRgba .width() / 3, mRgba .height() / 3, mRgba.width() / 3, mRgba.height() *5/ 8);
//    int left=width / 4+30;//1/3-->1/4
//    int top=height *3/16;
//    int new_width=width *1/2-60 ;//1/3-->1/2
//    int new_height=height *11/ 16;//5/8-->6/8
//
//    //转换-算法
//    if(VP_NATIVE_X0>0){
//        left=width / 4+30- (width/2-VP_NATIVE_X0);
//        if(left<0) left=0;
//        if(left+new_width> width) {
//            left=width-new_width-5;
//        }
//    }
//    if(VP_NATIVE_Y0>0 ){
//        top=height*3  /16-( height/2-VP_NATIVE_Y0);
//        if(top<0) top=0;
//        if(top+new_height>height){
//            top=height-new_height-5;
//        }
//    }


    int left=width *1/2-200;//1/3-->1/4
    int top=height *1/2-100;
    int new_width=400 ;//1/3-->1/2
    int new_height=300;//5/8-->6/8

    //转换-算法
    if(VP_NATIVE_X0>0){
        left=VP_NATIVE_X0-200;
        top=VP_NATIVE_Y0-100;
        if(left<0 ) left=0;
        if(top<0)   top=0;

        if(left+new_width>=width) left=width-new_width;
        if(top+new_height>=height)   top=height-new_height;
    }

    cv::Rect roi(left,top,new_width,new_height);
    cv::Mat roi_image=tmp_frame(roi);
    cv::resize(roi_image,roi_image,cv::Size(),ratio,ratio);
    Detection::View image = roi_image;
   //--------------------------------------------------------------------
    int left0=width *1/2-150;//1/3-->1/4
    int top0=height *1/2-100;
    int new_width0=300 ;//1/3-->1/2
    int new_height0=300;//5/8-->6/8

    //转换-算法
    if(VP_NATIVE_X0>0){
        left0=VP_NATIVE_X0-150;
        top0=VP_NATIVE_Y0-100;
        if(left0<0 ) left=0;
        if(top0<0)   top=0;
        if(left0+new_width0>=width) left0=width-new_width0;
        if(top0+new_height0>=height)   top0=height-new_height0;
    }
    cv::Rect roi0(left0,top0,new_width0,new_height0);
    cv::Mat roi_image0=tmp_frame(roi0);

    cv::resize(roi_image0,roi_image0,cv::Size(),ratio0,ratio0);
    Detection::View image0 = roi_image0;
    //-----------------------------------------------------------------------
    if(!isInit){
        isInit=true;
//          detection.Init(image.Size(),1.16,Detection::Size(3,3),Detection::Size(360,360),Detection::View(),3);
        detectionSmall.Init(image0.Size(),1.16,Detection::Size(3,3),Detection::Size(300,300),Detection::View(),12);
    }

    double timeStart = (double)getTickCount();
    Detection::Objects objects;
    detection.Detect(image, objects);

    Detection::Objects objects0;
    detectionSmall.Detect(image0, objects0);

    double Time = ((double)getTickCount() - timeStart) / getTickFrequency() * 1000;
    Json::Value root;
    Json::Value jVPointArray;
    for(int i=0;i<objects.size();i++)
    {
        cv::Rect bbox=simd2cv(objects[i].rect);
        bbox.x/=ratio;
        bbox.y/=ratio;
        bbox.width/=ratio;
        bbox.height/=ratio;

        int tl_x = bbox.tl().x+left;
        int tl_y = bbox.tl().y+top;
        int br_x = bbox.br().x+left;
        int br_y = bbox.br().y+top;
        Json::Value jVPoint;

        if(tl_x>left0 && br_x<left0+200){
        }else{
            jVPoint["x0"]=tl_x;
            jVPoint["y0"]=tl_y;
            jVPoint["x1"]=br_x;
            jVPoint["y1"]=br_y;
            jVPointArray.append(jVPoint);
        }
    }

    for(int i=0;i<objects0.size();i++)
    {
        cv::Rect bbox=simd2cv(objects0[i].rect);
        bbox.x/=ratio0;
        bbox.y/=ratio0;
        bbox.width/=ratio0;
        bbox.height/=ratio0;

        int tl_x = bbox.tl().x+left0;
        int tl_y = bbox.tl().y+top0;
        int br_x = bbox.br().x+left0;
        int br_y = bbox.br().y+top0;
        Json::Value jVPoint;

        jVPoint["x0"]=tl_x;
        jVPoint["y0"]=tl_y;
        jVPoint["x1"]=br_x;
        jVPoint["y1"]=br_y;
        jVPointArray.append(jVPoint);
    }

    root["carPoint"] = jVPointArray;
    root["time"] = Time;

    tmp_frame.release();
    return root;

}

Json::Value  test_it_half(Mat tmp_frame)
{

    cv::Mat frame;
    float ratio=0.3;
    if (tmp_frame.empty())
        return "";
    int height=tmp_frame.rows;
    int width=tmp_frame.cols;

    //roi区域保持一致
    //Rect rect = new Rect(mRgba .width() / 3, mRgba .height() / 3, mRgba.width() / 3, mRgba.height() *5/ 8);
    int left=width / 4;//1/3-->1/4
    int top=height /16;
    int new_width=width *1/2 ;//1/3-->1/2
    int new_height=height *14/ 16;//5/8-->6/8

    //转换-算法
    if(VP_NATIVE_X0>0){
        left=width / 4- (width/2-VP_NATIVE_X0);
        if(left<0) left=0;
        if(left+new_width> width) {
            left=width-new_width-5;
        }
    }
    if(VP_NATIVE_Y0>0 ){
        top=height /16-( height/2-VP_NATIVE_Y0);
        if(top<0) top=0;
        if(top+new_height>height){
            top=height-new_height-5;
        }
    }
    cv::Rect roi(left,top,new_width,new_height);
    cv::Mat roi_image=tmp_frame(roi);
    tmp_frame.release();
    cv::resize(roi_image,roi_image,cv::Size(),ratio,ratio);
    Detection::View image = roi_image;
    //1.15--1.18---1.2
    if(!isInitHalf){
        isInitHalf=true;
        detectionHalf.Init(image.Size(),1.16,Detection::Size(5,5),Detection::Size(150,75),Detection::View(),2);
    }
    double timeStart = (double)getTickCount();
    Detection::Objects objects;
    detectionHalf.Detect(image, objects);

    double Time = ((double)getTickCount() - timeStart) / getTickFrequency() * 1000;
    Json::Value root;
    Json::Value jVPointArray;
    for(int i=0;i<objects.size();i++)
    {
        cv::Rect bbox=simd2cv(objects[i].rect);
        bbox.x/=ratio;
        bbox.y/=ratio;
        bbox.width/=ratio;
        bbox.height/=ratio;

        int tl_x = bbox.tl().x+left;
        int tl_y = bbox.tl().y+top;
        int br_x = bbox.br().x+left;
        int br_y = bbox.br().y+top;
        Json::Value jVPoint;

        jVPoint["x0"]=tl_x;
        jVPoint["y0"]=tl_y;
        jVPoint["x1"]=br_x;
        jVPoint["y1"]=br_y;
        jVPointArray.append(jVPoint);
    }
    root["carPoint"] = jVPointArray;
    root["time"] = Time;
//  std::string out= root.toStyledString();
    return root;
}


JNIEXPORT void JNICALL Java_tianjin_calmcar_calmcar_1adas_AdasCarLaneWarper_setVPParaNew
        (JNIEnv *, jobject, jfloat vp_x, jfloat vp_y){
    VP_NATIVE_X0=vp_x;
    VP_NATIVE_Y0=vp_y;
}
//---------------------------------车道线body---------------------------------------start

JNIEXPORT jint JNICALL Java_tianjin_calmcar_calmcar_1adas_AdasCarLaneWarper_laneInitNew
        (JNIEnv *, jobject)
{
    chdir("/mnt/sdcard/test/");
    if(roadMarkingDetection!=NULL)
        return 0;

    else
    {
        roadMarkingDetection=new rmd::RoadMarkingDetection();
        roadMarkingDetection->Init("config.txt", kImageWidth, kImageHeight);
        std::cout << roadMarkingDetection->GetRoadMarkDetectionVersion() << std::endl;
        return 1;
    }
}
JNIEXPORT void JNICALL Java_tianjin_calmcar_calmcar_1adas_AdasCarLaneWarper_laneReleaseNew
        (JNIEnv *, jobject)
{
    if(roadMarkingDetection!=NULL)
    {
        delete roadMarkingDetection;
        roadMarkingDetection=NULL;
    }
}

string Int_to_String01(int n)
{

    ostringstream stream;

    stream<<n; //n为int类型

    return stream.str();

}
int mm=0;
unsigned long GetTickCount()
{
    struct timespec ts;

    clock_gettime(CLOCK_MONOTONIC, &ts);

    return (ts.tv_sec * 1000 + ts.tv_nsec / 1000000);
}


JNIEXPORT jstring JNICALL Java_tianjin_calmcar_calmcar_1adas_AdasCarLaneWarper_laneCarDetectNew
        (JNIEnv *env, jobject obj, jbyteArray image, jint height, jint width,jfloat rate )
{
    if(licenceEnable==0) return NULL;
    Json::Value root;
    jbyte* image_data = (jbyte*)env->GetByteArrayElements(image,0);
    jsize data_length = env->GetArrayLength(image);
    cv::Mat rgb_image(height, width, CV_8UC1, image_data);
    Json::Value   laneResult;
    Json::Value   carResult;
    if(LANE_DETECT_OPEN==1){
         laneResult=test_lane(rgb_image);
    }
    if(CAR_DETECT_OPEN==1){
        if(rate< 3.6 ) {
            root["carType"] = 1;
            carResult=test_it_half(rgb_image);
        }else{
            root["carType"] = 1;
            carResult=test_it(rgb_image);
        }
    }
    root["carData"] = carResult;
    root["laneData"] = laneResult;
    std::string out=  root.toStyledString();
    jstring result_json = env->NewStringUTF( out.c_str());
    env->ReleaseByteArrayElements(image,image_data,JNI_OK);
    rgb_image.release();
    return result_json;

}

JNIEXPORT jstring JNICALL Java_tianjin_calmcar_calmcar_1adas_AdasCarLaneWarper_laneCarDetectNew2
        (JNIEnv *env, jobject obj, jlong  imageAddr, jint height, jint width,jfloat rate )
{
//    if(licenceEnable==0) return NULL;
    Json::Value root;
//    jbyte* image_data = (jbyte*)env->GetByteArrayElements(image,0);
//    jsize data_length = env->GetArrayLength(image);
//    cv::Mat rgb_image(height, width, CV_8UC1, image_data);
    Mat* src = (Mat*)imageAddr;
    cv::Mat rgb_image=*src;

    Json::Value   laneResult;
    Json::Value   carResult;
    if(LANE_DETECT_OPEN==1){
        laneResult=test_lane(rgb_image);
    }
    if(CAR_DETECT_OPEN==1){
        if(rate< 3.6 ) {
            root["carType"] = 1;
            carResult=test_it_half(rgb_image);
        }else{
            root["carType"] = 1;
            carResult=test_it_for_big_and_small(rgb_image);
//            carResult=test_it_half(rgb_image);
        }
    }
    root["carData"] = carResult;
    root["laneData"] = laneResult;
    std::string out=  root.toStyledString();
    jstring result_json = env->NewStringUTF( out.c_str());
//    env->ReleaseByteArrayElements(image,image_data,JNI_OK);
    rgb_image.release();
    return result_json;

}


Json::Value  test_lane(Mat rgb_image)
{
    cv::resize(rgb_image,rgb_image,cv::Size(kImageWidth,kImageHeight));
    std::cout << roadMarkingDetection->GetRoadMarkDetectionVersion() << std::endl;
    long t = GetTickCount();
    CALMCARLDWDATA outResult = roadMarkingDetection->Update(rgb_image);

    t = GetTickCount() - t;
    Json::Value  json_str = roadMarkingDetection->createResultJson(outResult,t);

    return json_str;
}

JNIEXPORT jstring JNICALL Java_tianjin_calmcar_calmcar_1adas_AdasCarLaneWarper_getKimiStrNew(JNIEnv * env , jobject) {
    std::string kmiStr =  "AKl18Gpf";
    return env->NewStringUTF(kmiStr .c_str());
}


JNIEXPORT void JNICALL Java_tianjin_calmcar_calmcar_1adas_AdasCarLaneWarper_setLDWParameterNew
        (JNIEnv *, jobject ,jshort ldwMinDis  ,jfloat ldwThreadK )  //short ldwMinDis,float ldwThreadK);
{
    if(roadMarkingDetection!=NULL)
    {
        roadMarkingDetection->setLdwParameter(ldwMinDis ,ldwThreadK);

    }
}
JNIEXPORT jint JNICALL Java_tianjin_calmcar_calmcar_1adas_AdasCarLaneWarper_passNew
        (JNIEnv *env, jobject, jstring str,jstring avCode){
    const char *originStr;
    const char *avCodeStr;
    const char *sdkStr="96k018515hk";
    //将jstring转化成char *类型
    originStr = env->GetStringUTFChars(str, JNI_FALSE);
    char *resStr=join2(sdkStr,originStr);
    MD5 md5 = MD5(resStr);
    std::string md5Result = md5.hexdigest();

    avCodeStr=env->GetStringUTFChars(avCode, JNI_FALSE);
    std:: string avCodeString = avCodeStr;
    int mm=md5Result.compare(avCodeString);
    //将char *类型转化成jstring返回给Java层
    if(mm==0){
        licenceEnable=1;
    }else{
        licenceEnable=0;
    }
    return mm;
}

JNIEXPORT void JNICALL Java_tianjin_calmcar_calmcar_1adas_AdasCarLaneWarper_setVPParaLaneNew
        (JNIEnv *, jobject, jfloat vp_x, jfloat vp_y){
    roadMarkingDetection->setVanishPoint(vp_x,vp_y);
}

JNIEXPORT void JNICALL Java_tianjin_calmcar_calmcar_1adas_AdasCarLaneWarper_setDetectStateNew
        (JNIEnv *, jobject, jint laneState, jint carState){
    if(carState>=0){
        CAR_DETECT_OPEN=carState;
    }
    if(laneState>=0){
        LANE_DETECT_OPEN=laneState;
    }
}

//前车启动-------移动检测逻辑start

Json::Value  test_move(Mat tmp_frame)
{

    cv::Mat frame;
    float ratio=0.4;
    if (tmp_frame.empty())
        return "";
    int height=tmp_frame.rows;
    int width=tmp_frame.cols;


    //roi区域保持一致
    //Rect rect = new Rect(mRgba .width() / 3, mRgba .height() / 3, mRgba.width() / 3, mRgba.height() *5/ 8);
    int left=width / 4+30;//1/3-->1/4
    int top=height *3/16;
    int new_width=width *1/2-60 ;//1/3-->1/2
    int new_height=height *11/ 16;//5/8-->6/8

    //转换-算法
    if(VP_NATIVE_X0>0){
        left=width / 4+30- (width/2-VP_NATIVE_X0);
        if(left<0) left=0;
        if(left+new_width> width) {
            left=width-new_width-5;
        }
    }
    if(VP_NATIVE_Y0>0 ){
        top=height*3  /16-( height/2-VP_NATIVE_Y0);
        if(top<0) top=0;
        if(top+new_height>height){
            top=height-new_height-5;
        }
    }

    cv::Rect roi(left,top,new_width,new_height);
    cv::Mat roi_image=tmp_frame(roi);
    tmp_frame.release();
    cv::resize(roi_image,roi_image,cv::Size(),ratio,ratio);
    Detection::View image = roi_image;
    //1.15--1.18---1.2
//    if(!isInit){
//        isInit=true;
//        detection.Init(image.Size(),1.16,Detection::Size(8,8),Detection::Size(300,300),Detection::View(),2);
//    }
    //1.15--1.18---1.2
    if(!isInit){
        isInit=true;
        detection.Init(image.Size(),1.17,Detection::Size(7,7),Detection::Size(200,200),Detection::View(),4);
    }
    double timeStart = (double)getTickCount();
    Detection::Objects objects;
    detection.Detect(image, objects);

    double Time = ((double)getTickCount() - timeStart) / getTickFrequency() * 1000;
    Json::Value root;
    Json::Value jVPointArray;
    for(int i=0;i<objects.size();i++)
    {
        cv::Rect bbox=simd2cv(objects[i].rect);
        bbox.x/=ratio;
        bbox.y/=ratio;
        bbox.width/=ratio;
        bbox.height/=ratio;

        int tl_x = bbox.tl().x+left;
        int tl_y = bbox.tl().y+top;
        int br_x = bbox.br().x+left;
        int br_y = bbox.br().y+top;
        Json::Value jVPoint;

        jVPoint["x0"]=tl_x;
        jVPoint["y0"]=tl_y;
        jVPoint["x1"]=br_x;
        jVPoint["y1"]=br_y;
        jVPointArray.append(jVPoint);
    }
    root["carPoint"] = jVPointArray;
    root["time"] = Time;
//  std::string out= root.toStyledString();
    return root;






}
















//前车启动-------移动检测逻辑end
//---------------------------------车道线body---------------------------------------end

